//EXPERIMENTAL
#include <ros/ros.h>
#include <robina_mapmarker/markPoseStamped.h>
#include <robina_mapmarker/getPoseState.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <tf/transform_listener.h>
#include <cv_bridge/cv_bridge.h>
#include <cv_bridge/cv_bridge.h>
#include <occupancy_grid_utils/coordinate_conversions.h>
#include <occupancy_grid_utils/combine_grids.h>
#include <ros/message.h>
#include <iostream>
#include <visualization_msgs/InteractiveMarker.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <visualization_msgs/InteractiveMarkerControl.h>
#include <visualization_msgs/InteractiveMarkerInit.h>
#include <interactive_markers/interactive_marker_server.h>

nav_msgs::MapMetaData mmd;

void processFeedback(
        const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
    nav_msgs::OccupancyGrid ocg;
    occupancy_grid_utils::Cell cell;
    geometry_msgs::Point point;
    point = feedback->pose.position;
    //point.y *= -1;
    try
    {
        ocg = *ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("/map");
        cell = occupancy_grid_utils::pointCell(mmd,point);
        char data = ocg.data[cellIndex(ocg.info, cell)];
        std::cout<<(int)data<<std::endl;
    }
    catch(occupancy_grid_utils::GridUtilsException &e)
    {
        std::cout<<"Exception "<<e.what()<<"\n";
    }
}
using namespace std;

class MapMarker
{
//    sensor_msgs::CvBridge bridge_;
    ros::NodeHandle nh_;
    image_transport::ImageTransport    it_;
    ros::Publisher                     pub_;
    tf::TransformListener              tf_listener_;
    CvFont                             font_;
    ros::ServiceServer                 markPose_server;
    ros::ServiceServer                 getPoseState_server;
    IplImage*                          currentImage;
    std::string                        map_path;
    nav_msgs::MapMetaData              mapmetadata;
public:
    MapMarker()
        : it_(nh_)
    {
        nh_.param<std::string>("map_path",map_path,"/home/athome/emergency_map.pgm");

        pub_ = nh_.advertise<sensor_msgs::Image>("map_out", 1);

        markPose_server = nh_.advertiseService("markPose",&MapMarker::markPose_serviceCB,this);
        getPoseState_server = nh_.advertiseService("getPoseState",&MapMarker::getPoseState_serviceCB,this);
        ROS_INFO("Waiting for map_metadata");
        mapmetadata = *ros::topic::waitForMessage<nav_msgs::MapMetaData>("/map_metadata");
        mmd = getMapMetaData();
        ROS_INFO("map_metadata received");
        currentImage = cvLoadImage(map_path.c_str());
        ROS_INFO("Map image have been loaded");
    }
    nav_msgs::MapMetaData getMapMetaData(){return mapmetadata;}

    //EXPERIMENTAL
    bool getPoseState_serviceCB(robina_mapmarker::getPoseState::Request &req,
                                robina_mapmarker::getPoseState::Response &res)
    {
        nav_msgs::OccupancyGrid ocg;
        occupancy_grid_utils::Cell cell;
        geometry_msgs::Point point;
        point = req.pose.pose.position;
        //point.y *= -1;
        try
        {
            ocg = *ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("/map");
            cell = occupancy_grid_utils::pointCell(mmd,point);
            char data = ocg.data[cellIndex(ocg.info, cell)];
            std::cout<<(int)data<<std::endl;
            switch((int)data)
            {
            case 0:
                res.state = robina_mapmarker::getPoseState::Response::NOTOCCUPIED;
                break;
            case 100:
                res.state = robina_mapmarker::getPoseState::Response::OCCUPIED;
                break;
            case -1:
                res.state = robina_mapmarker::getPoseState::Response::UNKNOWN;
                break;
            default:
                ROS_ERROR("Something is wrong");
                return false;
            }
        }
        catch(occupancy_grid_utils::GridUtilsException &e)
        {
            std::cout<<"Exception "<<e.what()<<"\n";
            return false;
        }

        return true;
    }

    bool markPose_serviceCB( robina_mapmarker::markPoseStamped::Request &req,
                             robina_mapmarker::markPoseStamped::Response &res )
    {
        ROS_INFO("Request received");
        if(req.markerPose.header.frame_id!="/map")
        {
            try
            {
                if( tf_listener_.waitForTransform( "/map", req.markerPose.header.frame_id, ros::Time(0), ros::Duration(10,0) ) )
                {
                    tf_listener_.transformPose("/map",req.markerPose, req.markerPose);
                }
            }
            catch (tf::TransformException& ex)
            {
                ROS_WARN("[draw_frames] TF exception:\n%s", ex.what());
                return false;
            }
        }
        occupancy_grid_utils::Cell cell;
        geometry_msgs::Point point;
        req.markerPose.pose.position.y*=-1;
        point = req.markerPose.pose.position;
        cell = occupancy_grid_utils::pointCell(mapmetadata,point);
        nav_msgs::OccupancyGrid s;

        cv::Point2d uv(cell.x,
                       cell.y);

        CvPoint origin = cvPoint(uv.x,
                                 uv.y);

        static const int RADIUS = 3;
        cvCircle(currentImage, uv, RADIUS, CV_RGB(255,0,0), -1);
        cvInitFont(&font_, CV_FONT_HERSHEY_SIMPLEX, 0.3, 0.3);
        stringstream ss;
        string str;
        switch(req.markerType)
        {
        case robina_mapmarker::markPoseStamped::Request::fireLocation:
            cvPutText(currentImage, "fireLocation", origin, &font_, CV_RGB(0,0,255));
            ROS_INFO("FireLocation");
            break;
        case robina_mapmarker::markPoseStamped::Request::location:
            cvPutText(currentImage, "location", origin, &font_, CV_RGB(255,255,0));
            break;
        case robina_mapmarker::markPoseStamped::Request::person:
            cvPutText(currentImage, "healthy", origin, &font_, CV_RGB(0,255,0));
            break;
        case robina_mapmarker::markPoseStamped::Request::victim:
            ss<<"victim "<<req.index;
            cvPutText(currentImage,ss.str().c_str(), origin, &font_, CV_RGB(255,0,0));
            ss.flush();
            ROS_INFO("ViCTIM");
            break;
        default:
            cvPutText(currentImage, "unknown", origin, &font_, CV_RGB(241,234,0));
            break;
        }

        try
        {
            cvSaveImage(map_path.c_str(),currentImage);
        }
        catch(cv::Exception e)
        {
            std::cout<<e.what();
            res.result = false;
            return false;
        }
        res.result = true;

        cv_bridge::CvImage bridge;

        bridge.image = currentImage;

        pub_.publish(bridge);

        return true;
    }

};

int main(int argc,char** argv)
{
    ros::init(argc, argv, "robina_mapmarker");
    MapMarker mapMarker;
    interactive_markers::InteractiveMarkerServer server("s_marker");
    visualization_msgs::InteractiveMarker interactiveMarker;
    interactiveMarker.header.frame_id = "/map";
    interactiveMarker.name = "cell-checker";
    visualization_msgs::Marker box_marker;
    box_marker.type = visualization_msgs::Marker::ARROW;
    box_marker.scale.x = 2.0;
    box_marker.scale.y = 2.0;
    box_marker.scale.z = 0.45;
    box_marker.color.r = 0.005;
    box_marker.color.g = 1.0;
    box_marker.color.b = 0.05;
    box_marker.color.a = 1.0;
    visualization_msgs::InteractiveMarkerControl box_control;
    box_control.always_visible = true;
    box_control.markers.push_back(box_marker);
    interactiveMarker.controls.push_back(box_control);
    visualization_msgs::InteractiveMarkerControl rotate_control;
    rotate_control.name = "move_x";
    rotate_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    interactiveMarker.controls.push_back(rotate_control);
    rotate_control.name = "move_y";
    rotate_control.orientation.w = 1;
    rotate_control.orientation.x = 0;
    rotate_control.orientation.y = 0;
    rotate_control.orientation.z = 1;
    rotate_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    interactiveMarker.controls.push_back(rotate_control);
    server.insert(interactiveMarker, &processFeedback);
    server.applyChanges();
    ros::spin();
    ros::waitForShutdown();
}

